#include "Matrix.h"
#include <iostream>
#include <cmath>

using namespace std;

Matrix::Matrix()
{
	for (int i=0; i<M_SIZE; i++)
		for (int j=0; j<M_SIZE; j++)
			tab[i][j]=0;
}

Matrix::Matrix(const Vector &v1, const Vector &v2,const Vector &v3, const Point &p)
{
	tab[0][0] = v1.get_x();
	tab[0][1] = v1.get_y();
	tab[0][2] = v1.get_z();
	tab[0][0] = 0.0;
	
	tab[1][0] = v2.get_x();
	tab[1][1] = v2.get_y();
	tab[1][2] = v2.get_z();
	tab[1][0] = 0.0;

	tab[2][0] = v3.get_x();
	tab[2][1] = v3.get_y();
	tab[2][2] = v3.get_z();
	tab[2][0] = 0.0;

	tab[3][0] = p.get_x();
	tab[3][1] = p.get_y();
	tab[3][2] = p.get_z();
	tab[3][0] = 1.0;
}

Matrix::Matrix(const Matrix &m)
{
	for (int i=0; i<M_SIZE; i++)
		for (int j=0; j<M_SIZE; j++)
			tab[i][j]=m.get_tab(i, j);
}

Matrix Matrix::operator+(const Matrix &m) const
{
	Matrix res = Matrix();
	for (int i=0; i<M_SIZE; i++)
		for (int j=0; j<M_SIZE; j++)
			res.set_tab(i, j, m.get_tab(i, j) + get_tab(i, j));
	return res;
}
Matrix Matrix::operator*(const Matrix &m) const
{
	Matrix res = Matrix();
	for (int i=0; i<M_SIZE; i++)
		for (int j=0; j<M_SIZE; j++)
			for (int k=0; k<M_SIZE; k++)
				res.set_tab(i, j, res.get_tab(i, j) + m.get_tab(i, k) * get_tab(k, j));
	return res;
}
Vector Matrix::operator*(const Vector &v) const
{
	Vector res = Vector();
	res.set_x(get_tab(0,0)*v.get_x()+get_tab(0,1)*v.get_y()+get_tab(0,2)*v.get_z()+get_tab(0,3)*v.get_t());
	res.set_y(get_tab(1,0)*v.get_x()+get_tab(1,1)*v.get_y()+get_tab(1,2)*v.get_z()+get_tab(1,3)*v.get_t());
	res.set_z(get_tab(2,0)*v.get_x()+get_tab(2,1)*v.get_y()+get_tab(2,2)*v.get_z()+get_tab(2,3)*v.get_t());
	res.set_t(get_tab(3,0)*v.get_x()+get_tab(3,1)*v.get_y()+get_tab(3,2)*v.get_z()+get_tab(3,3)*v.get_t());
	return res;
}

Point Matrix::operator*(const Point &p) const
{
	Point res = Point();
	res.set_x(get_tab(0,0)*p.get_x()+get_tab(0,1)*p.get_y()+get_tab(0,2)*p.get_z()+get_tab(0,3)*p.get_t());
	res.set_y(get_tab(1,0)*p.get_x()+get_tab(1,1)*p.get_y()+get_tab(1,2)*p.get_z()+get_tab(1,3)*p.get_t());
	res.set_z(get_tab(2,0)*p.get_x()+get_tab(2,1)*p.get_y()+get_tab(2,2)*p.get_z()+get_tab(2,3)*p.get_t());
	res.set_t(get_tab(3,0)*p.get_x()+get_tab(3,1)*p.get_y()+get_tab(3,2)*p.get_z()+get_tab(3,3)*p.get_t());
	return res;
}

/*Point Matrix::multPoint(const Point &p) const
{
	Point res = Point();
	res.set_x(get_tab(0,0)*p.get_x()+get_tab(1,0)*p.get_y()+get_tab(2,0)*p.get_z()+get_tab(3,0)*p.get_t());
	res.set_y(get_tab(0,1)*p.get_x()+get_tab(1,1)*p.get_y()+get_tab(2,1)*p.get_z()+get_tab(3,1)*p.get_t());
	res.set_z(get_tab(0,2)*p.get_x()+get_tab(1,2)*p.get_y()+get_tab(2,2)*p.get_z()+get_tab(3,2)*p.get_t());
	res.set_t(get_tab(0,3)*p.get_x()+get_tab(1,3)*p.get_y()+get_tab(2,3)*p.get_z()+get_tab(3,3)*p.get_t());
	return res;
}*/

Matrix Matrix::translation(const Vector &v)
{
	Matrix m = Matrix();
	for (int i=0; i<M_SIZE; i++)
		m.set_tab(i, i, 1.0);
	
	m.set_tab(0, 3, v.get_x());
	m.set_tab(1, 3, v.get_y());
	m.set_tab(2, 3, v.get_z());
	
	return m;
}

Matrix Matrix::rotation_x(const double angle)
{
	Matrix m = Matrix();
	for (int i=0; i<M_SIZE; i++)
		m.set_tab(i, i, 1.0);
	
	m.set_tab(1, 1, cos(angle));
	m.set_tab(2, 1, -sin(angle));
	m.set_tab(1, 2, sin(angle));
	m.set_tab(2, 2, cos(angle));
	
	return m;
}

Matrix Matrix::rotation_y(const double angle)
{
	Matrix m = Matrix();
	for (int i=0; i<M_SIZE; i++)
		m.set_tab(i, i, 1.0);
	
	m.set_tab(0, 0, cos(angle));
	m.set_tab(2, 0, sin(angle));
	m.set_tab(0, 2, -sin(angle));
	m.set_tab(2, 2, cos(angle));
	
	return m;
}

Matrix Matrix::rotation_z(const double angle)
{
	Matrix m = Matrix();
	for (int i=0; i<M_SIZE; i++)
		m.set_tab(i, i, 1.0);
	
	m.set_tab(0, 0, cos(angle));
	m.set_tab(1, 0, -sin(angle));
	m.set_tab(0, 1, sin(angle));
	m.set_tab(1, 1, cos(angle));
	
	return m;
}

Matrix Matrix::scopeChange(const Point &p, const Vector &direction)
{
	Matrix m = Matrix();
	Vector v(direction);
	v.normalize();
	Vector vup(0, 1, 0);
	Vector u= v ^ vup;
	if (u.norm() == 0) {
		vup = Vector (0, 0, 1);
		u= v ^ vup;
	}
	Vector vup2= v ^ u;

	u.normalize();
	vup2.normalize();
	
	m.set_tab(0,0,u.get_x());
	m.set_tab(0,1,u.get_y());
	m.set_tab(0,2,u.get_z());
	
	m.set_tab(1,0,vup2.get_x());
	m.set_tab(1,1,vup2.get_y());
	m.set_tab(1,2,vup2.get_z());
	
	m.set_tab(2,0,v.get_x());
	m.set_tab(2,1,v.get_y());
	m.set_tab(2,2,v.get_z());
	
	m.set_tab(3,0,p.get_x());
	m.set_tab(3,1,p.get_y());
	m.set_tab(3,2,p.get_z());
	m.set_tab(3,3,1.0);
	
	return(m);
}

Matrix Matrix::transposed() const
{
	Matrix m = Matrix();
	for (int i=0; i<M_SIZE; i++)
		for (int j=0; j<M_SIZE; j++)
				m.set_tab(j, i, this->get_tab(i, j));
	return m;
}

void Matrix::display() const
{
	for (int i=0; i<M_SIZE; i++)
	{
		for (int j=0; j<M_SIZE; j++)
				cout << this->get_tab(i, j) << "\t";
		cout << endl;
	}
}
